<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>Robocup-denayer-project-a: read-only/trunk/source/besturing.cpp Bron Bestand</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javaScript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css"/>
</head>
<body>
<!-- Gegenereerd door Doxygen 1.7.1 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Zoeken');
--></script>
<script type="text/javascript">
function hasClass(ele,cls) {
  return ele.className.match(new RegExp('(\\s|^)'+cls+'(\\s|$)'));
}

function addClass(ele,cls) {
  if (!this.hasClass(ele,cls)) ele.className += " "+cls;
}

function removeClass(ele,cls) {
  if (hasClass(ele,cls)) {
    var reg = new RegExp('(\\s|^)'+cls+'(\\s|$)');
    ele.className=ele.className.replace(reg,' ');
  }
}

function toggleVisibility(linkObj) {
 var base = linkObj.getAttribute('id');
 var summary = document.getElementById(base + '-summary');
 var content = document.getElementById(base + '-content');
 var trigger = document.getElementById(base + '-trigger');
 if ( hasClass(linkObj,'closed') ) {
   summary.style.display = 'none';
   content.style.display = 'block';
   trigger.src = 'open.png';
   removeClass(linkObj,'closed');
   addClass(linkObj,'opened');
 } else if ( hasClass(linkObj,'opened') ) {
   summary.style.display = 'block';
   content.style.display = 'none';
   trigger.src = 'closed.png';
   removeClass(linkObj,'opened');
   addClass(linkObj,'closed');
 }
 return false;
}
</script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Hoofd&nbsp;Pagina</span></a></li>
      <li><a href="annotated.html"><span>Klassen</span></a></li>
      <li class="current"><a href="files.html"><span>Bestanden</span></a></li>
      <li id="searchli">
        <div id="MSearchBox" class="MSearchBoxInactive">
          <span class="left">
            <form id="FSearchBox" action="search.php" method="get">
              <img id="MSearchSelect" src="search/mag.png" alt=""/>
              <input type="text" id="MSearchField" name="query" value="Zoeken" size="20" accesskey="S" 
                     onfocus="searchBox.OnSearchFieldFocus(true)" 
                     onblur="searchBox.OnSearchFieldFocus(false)"/>
            </form>
          </span><span class="right"></span>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&nbsp;Lijst</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;members</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<h1>read-only/trunk/source/besturing.cpp</h1>  </div>
</div>
<div class="contents">
<a href="besturing_8cpp.html">Ga naar de documentatie van dit bestand.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00011"></a>00011 <span class="preprocessor">#include &lt;iostream&gt;</span>
<a name="l00012"></a>00012 <span class="preprocessor">#include &lt;math.h&gt;</span>
<a name="l00013"></a>00013 <span class="preprocessor">#include &quot;<a class="code" href="besturing_8h.html">besturing.h</a>&quot;</span>
<a name="l00014"></a>00014 
<a name="l00015"></a>00015 <span class="keyword">using namespace </span>std;
<a name="l00016"></a>00016  
<a name="l00017"></a><a class="code" href="class_besturing.html#a5a5a117fc659778bfe47e2fd63e08c42">00017</a> <a class="code" href="class_besturing.html#a5a5a117fc659778bfe47e2fd63e08c42">Besturing::Besturing</a>(<span class="keywordtype">void</span>)
<a name="l00018"></a>00018 {
<a name="l00019"></a>00019     <span class="comment">// mouse left</span>
<a name="l00020"></a>00020     muisL=<span class="keyword">new</span> <a class="code" href="class_muis.html">Muis</a>(<span class="stringliteral">&quot;/dev/input/mouse1&quot;</span>);
<a name="l00021"></a>00021     muisL-&gt;<a class="code" href="class_muis.html#acd343ddb5fc59dc1d57b208c53396d12">setVerbose</a>(<span class="keyword">true</span>);
<a name="l00022"></a>00022     muisL-&gt;<a class="code" href="class_muis.html#adb79030b3314ed07c40d88c7597914bf">setDPI</a>(800);
<a name="l00023"></a>00023     muisL-&gt;<a class="code" href="class_muis.html#a9d8360975a2612654ae625f30433a260">setAngle</a>(M_PI);
<a name="l00024"></a>00024     muisL-&gt;<a class="code" href="class_thread.html#a1f53ee62bd30a7924186ef26150ce262" title="Thread::start();.">start</a>();
<a name="l00025"></a>00025     <span class="comment">// mouse right</span>
<a name="l00026"></a>00026     muisR=<span class="keyword">new</span> <a class="code" href="class_muis.html">Muis</a>(<span class="stringliteral">&quot;/dev/input/mouse2&quot;</span>);
<a name="l00027"></a>00027     muisR-&gt;<a class="code" href="class_muis.html#acd343ddb5fc59dc1d57b208c53396d12">setVerbose</a>(<span class="keyword">true</span>);
<a name="l00028"></a>00028     muisR-&gt;<a class="code" href="class_muis.html#adb79030b3314ed07c40d88c7597914bf">setDPI</a>(800);
<a name="l00029"></a>00029     muisR-&gt;<a class="code" href="class_muis.html#a9d8360975a2612654ae625f30433a260">setAngle</a>(M_PI);
<a name="l00030"></a>00030     muisR-&gt;<a class="code" href="class_thread.html#a1f53ee62bd30a7924186ef26150ce262" title="Thread::start();.">start</a>();
<a name="l00031"></a>00031     <span class="comment">// motors</span>
<a name="l00032"></a>00032     motors=<span class="keyword">new</span> <a class="code" href="class_motors.html">Motors</a>(0,19200);
<a name="l00033"></a>00033         <span class="comment">// be sure motors had stopped</span>
<a name="l00034"></a>00034         motors-&gt;<a class="code" href="class_motors.html#a2a03135d58d03d8a82be9be0ceca1236" title="stopMotors">stopMotors</a>();
<a name="l00035"></a>00035         <span class="comment">// set current location</span>
<a name="l00036"></a>00036         <a class="code" href="class_besturing.html#a216acb50c4b2a0efbd999203e4928d40">setLocation</a>(0,0,0.0);
<a name="l00037"></a>00037         <span class="comment">// set position matrix</span>
<a name="l00038"></a>00038         <span class="comment">//posMat =      {       {0.5000,        -0.5000,        0.5000,         0.5000  },</span>
<a name="l00039"></a>00039     <span class="comment">//                          {0.0000,        0.5000,         -0.0000,        0.5000  },</span>
<a name="l00040"></a>00040         <span class="comment">//                              {0.0000,        0.0125,         -0.0000,        -0.0125 }       }; </span>
<a name="l00041"></a>00041         posMat[0][0]=0.5;
<a name="l00042"></a>00042         posMat[0][1]=-0.5;
<a name="l00043"></a>00043         posMat[0][2]=0.5;
<a name="l00044"></a>00044         posMat[0][3]=0.5;
<a name="l00045"></a>00045         posMat[1][0]=0.0;
<a name="l00046"></a>00046         posMat[1][1]=0.5;
<a name="l00047"></a>00047         posMat[1][2]=0.0;
<a name="l00048"></a>00048         posMat[1][3]=0.5;
<a name="l00049"></a>00049         posMat[2][0]=0.0;
<a name="l00050"></a>00050         posMat[2][1]=0.0125;
<a name="l00051"></a>00051         posMat[2][2]=0.0;
<a name="l00052"></a>00052         posMat[2][3]=-0.0125;
<a name="l00053"></a>00053         
<a name="l00054"></a>00054         <span class="comment">// Muizen juist zetten</span>
<a name="l00055"></a>00055         <span class="keywordtype">int</span> oldXL,oldXR         ;
<a name="l00056"></a>00056         oldXL = muisL-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>();
<a name="l00057"></a>00057         oldXR = muisR-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>();
<a name="l00058"></a>00058         motors-&gt;<a class="code" href="class_motors.html#ae5fe2fd03098baf0fd6e55c1b52a01e9" title="setSpeed">setSpeed</a>(50,<a class="code" href="motors_8h.html#aecfc8cb31c77ff43036a88e475f0198d">MOTOR_FORWARD</a>,50,<a class="code" href="motors_8h.html#a66ddd712d0c88e8299bdfe5e04d8600f">MOTOR_REVERSE</a>);
<a name="l00059"></a>00059         usleep(500000);
<a name="l00060"></a>00060         motors-&gt;<a class="code" href="class_motors.html#ae5fe2fd03098baf0fd6e55c1b52a01e9" title="setSpeed">setSpeed</a>(0,<a class="code" href="motors_8h.html#adaa6428b3f3b92676ba73f27b1ed8035">MOTOR_BRAKE</a>,0,<a class="code" href="motors_8h.html#adaa6428b3f3b92676ba73f27b1ed8035">MOTOR_BRAKE</a>);
<a name="l00061"></a>00061         <span class="keywordflow">if</span>( (muisL-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>() - oldXL) &lt; (muisR-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>() - oldXR ))
<a name="l00062"></a>00062         {
<a name="l00063"></a>00063                 <a class="code" href="class_muis.html">Muis</a> *temp = muisL;
<a name="l00064"></a>00064                 muisL = muisR;
<a name="l00065"></a>00065                 muisR = temp;
<a name="l00066"></a>00066         }
<a name="l00067"></a>00067         <span class="comment">// trug rechtrijden</span>
<a name="l00068"></a>00068         motors-&gt;<a class="code" href="class_motors.html#ae5fe2fd03098baf0fd6e55c1b52a01e9" title="setSpeed">setSpeed</a>(50,<a class="code" href="motors_8h.html#a66ddd712d0c88e8299bdfe5e04d8600f">MOTOR_REVERSE</a>,50,<a class="code" href="motors_8h.html#aecfc8cb31c77ff43036a88e475f0198d">MOTOR_FORWARD</a>);
<a name="l00069"></a>00069         usleep(500000);
<a name="l00070"></a>00070         motors-&gt;<a class="code" href="class_motors.html#ae5fe2fd03098baf0fd6e55c1b52a01e9" title="setSpeed">setSpeed</a>(0,<a class="code" href="motors_8h.html#adaa6428b3f3b92676ba73f27b1ed8035">MOTOR_BRAKE</a>,0,<a class="code" href="motors_8h.html#adaa6428b3f3b92676ba73f27b1ed8035">MOTOR_BRAKE</a>);
<a name="l00071"></a>00071         
<a name="l00072"></a>00072 }
<a name="l00073"></a>00073 
<a name="l00074"></a><a class="code" href="class_besturing.html#ab776a244947750dbc85eb46883979791">00074</a> <a class="code" href="class_besturing.html#ab776a244947750dbc85eb46883979791">Besturing::~Besturing</a>()
<a name="l00075"></a>00075 {
<a name="l00076"></a>00076         cout &lt;&lt; <span class="stringliteral">&quot;besturing object vernield &quot;</span>&lt;&lt; endl;
<a name="l00077"></a>00077 }
<a name="l00078"></a>00078 
<a name="l00079"></a><a class="code" href="class_besturing.html#a8d7400ebc6b2fff1e5e100c3a8de91f6">00079</a> <span class="keywordtype">void</span> <a class="code" href="class_besturing.html#a8d7400ebc6b2fff1e5e100c3a8de91f6" title="Thread::execute();.">Besturing::execute</a>()
<a name="l00080"></a>00080 {
<a name="l00081"></a>00081         <span class="keywordtype">float</span> a[MAXMAT][MAXMAT];
<a name="l00082"></a>00082         <span class="keywordtype">float</span> a1=7*M_PI/4;
<a name="l00083"></a>00083         <span class="keywordtype">float</span> a2=5*M_PI/4;
<a name="l00084"></a>00084         <span class="keywordtype">float</span> R[MAXMAT][MAXMAT];
<a name="l00085"></a>00085         <span class="keywordtype">float</span> ta;
<a name="l00086"></a>00086         <span class="keywordtype">int</span>     oldRX,oldRY,oldLX,oldLY;
<a name="l00087"></a>00087         <span class="keywordtype">int</span>     dxl,dxr,dyl,dyr;
<a name="l00088"></a>00088         
<a name="l00089"></a>00089         <span class="keywordflow">while</span>(1)
<a name="l00090"></a>00090         {
<a name="l00091"></a>00091                 usleep(100000);
<a name="l00092"></a>00092                 <span class="comment">// get cur position</span>
<a name="l00093"></a>00093                 dxl=muisL-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>()-oldLX; oldLX=muisL-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>();
<a name="l00094"></a>00094                 dxr=muisR-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>()-oldRX; oldRX=muisR-&gt;<a class="code" href="class_muis.html#a410ea39e18c1170ba371a9ac44ca3ddf">getX</a>();
<a name="l00095"></a>00095                 dyl=muisL-&gt;<a class="code" href="class_muis.html#a240784255a8ee20a4f9ef85ac4a54fd2">getY</a>()-oldLY; oldLY=muisL-&gt;<a class="code" href="class_muis.html#a240784255a8ee20a4f9ef85ac4a54fd2">getY</a>();
<a name="l00096"></a>00096                 dyr=muisR-&gt;<a class="code" href="class_muis.html#a240784255a8ee20a4f9ef85ac4a54fd2">getY</a>()-oldRY; oldRY=muisR-&gt;<a class="code" href="class_muis.html#a240784255a8ee20a4f9ef85ac4a54fd2">getY</a>();
<a name="l00097"></a>00097                 a[0][0] = dxr*cos(a1) - dyr*sin(a1); 
<a name="l00098"></a>00098                 a[1][0] = dxr*sin(a1) + dyr*cos(a1); 
<a name="l00099"></a>00099                 a[2][0] = dxl*cos(a2) - dyl*sin(a2); 
<a name="l00100"></a>00100                 a[3][0] = dxl*sin(a2) + dyl*cos(a2);
<a name="l00101"></a>00101                 <a class="code" href="class_besturing.html#a2d5e31d6391452b931b0c46729a8634d" title="Vermenigvuldig arrays.">MatrixMultiply</a>(posMat,a,R,3,4,1);
<a name="l00102"></a>00102                 this-&gt;x=R[0][0];
<a name="l00103"></a>00103                 this-&gt;y=R[1][0];
<a name="l00104"></a>00104                 this-&gt;angle=R[2][0];
<a name="l00105"></a>00105                 <span class="comment">// recalculate target angle</span>
<a name="l00106"></a>00106                 <span class="keywordflow">if</span>(x==this-&gt;x) 
<a name="l00107"></a>00107                         ta = (this-&gt;angle &gt; 0 ) ? M_PI : -M_PI;
<a name="l00108"></a>00108                 <span class="keywordflow">else</span>
<a name="l00109"></a>00109                         ta=atan((y-this-&gt;y)/(x-this-&gt;x));
<a name="l00110"></a>00110                 <span class="keywordflow">if</span>(this-&gt;x &lt; x) ta+=M_PI;
<a name="l00111"></a>00111                 <span class="comment">// set moving parameters</span>
<a name="l00112"></a>00112                 <span class="comment">// move</span>
<a name="l00113"></a>00113         }
<a name="l00114"></a>00114 }
<a name="l00115"></a>00115 
<a name="l00122"></a><a class="code" href="class_besturing.html#a5fc2f2acafc8f4f26447ca3f529a5099">00122</a> <span class="keywordtype">void</span> <a class="code" href="class_besturing.html#a5fc2f2acafc8f4f26447ca3f529a5099" title="ga naar punt en draai u dan richting angle">Besturing::goTo</a>(<span class="keywordtype">int</span> x,<span class="keywordtype">int</span> y,<span class="keywordtype">float</span> angle)
<a name="l00123"></a>00123 {
<a name="l00124"></a>00124 
<a name="l00125"></a>00125 }
<a name="l00126"></a>00126 
<a name="l00127"></a><a class="code" href="class_besturing.html#a216acb50c4b2a0efbd999203e4928d40">00127</a> <span class="keywordtype">void</span> <a class="code" href="class_besturing.html#a216acb50c4b2a0efbd999203e4928d40">Besturing::setLocation</a>(<span class="keywordtype">int</span> x,<span class="keywordtype">int</span> y,<span class="keywordtype">float</span> angle)
<a name="l00128"></a>00128 {
<a name="l00129"></a>00129         this-&gt;x=x;
<a name="l00130"></a>00130         this-&gt;y=y;
<a name="l00131"></a>00131         this-&gt;angle=angle;
<a name="l00132"></a>00132 }
<a name="l00133"></a>00133 
<a name="l00134"></a><a class="code" href="class_besturing.html#a32264f54af9bf7a88550096e93119928">00134</a> <span class="keywordtype">bool</span> <a class="code" href="class_besturing.html#a32264f54af9bf7a88550096e93119928">Besturing::isBusy</a>()
<a name="l00135"></a>00135 {
<a name="l00136"></a>00136         <span class="keywordflow">return</span> busy;
<a name="l00137"></a>00137 }
<a name="l00138"></a>00138 
<a name="l00139"></a><a class="code" href="class_besturing.html#aac7a1e0be9fe2f6fdab6e50e7f663b3a">00139</a> <span class="keywordtype">bool</span> <a class="code" href="class_besturing.html#aac7a1e0be9fe2f6fdab6e50e7f663b3a">Besturing::isReady</a>()
<a name="l00140"></a>00140 {
<a name="l00141"></a>00141         <span class="keywordflow">return</span> ready;
<a name="l00142"></a>00142 }
<a name="l00143"></a>00143 
<a name="l00153"></a><a class="code" href="class_besturing.html#a2d5e31d6391452b931b0c46729a8634d">00153</a> <span class="keywordtype">bool</span> <a class="code" href="class_besturing.html#a2d5e31d6391452b931b0c46729a8634d" title="Vermenigvuldig arrays.">Besturing::MatrixMultiply</a>(<span class="keywordtype">float</span> A[<a class="code" href="besturing_8h.html#a9ff476c1139fb6a63d420b961ec849f1" title="Besturing.">MAXMAT</a>][<a class="code" href="besturing_8h.html#a9ff476c1139fb6a63d420b961ec849f1" title="Besturing.">MAXMAT</a>],<span class="keywordtype">float</span> B[MAXMAT][MAXMAT],<span class="keywordtype">float</span> R[MAXMAT][MAXMAT],<span class="keywordtype">int</span> M,<span class="keywordtype">int</span> L,<span class="keywordtype">int</span> N)
<a name="l00154"></a>00154 {       
<a name="l00155"></a>00155         <span class="keywordtype">int</span> row,col,i;
<a name="l00156"></a>00156         <span class="keywordtype">float</span> sum;
<a name="l00157"></a>00157         <span class="keywordflow">for</span>(row=0;row&lt;M;row++)
<a name="l00158"></a>00158         {
<a name="l00159"></a>00159                 <span class="keywordflow">for</span>(col=0;col&lt;N;col++)
<a name="l00160"></a>00160                 {
<a name="l00161"></a>00161                         sum=0;
<a name="l00162"></a>00162                         <span class="keywordflow">for</span>(i=0;i&lt;L;i++)
<a name="l00163"></a>00163                                 sum+=A[row][i]*B[i][col];
<a name="l00164"></a>00164                         R[row][col]=sum;
<a name="l00165"></a>00165                 }
<a name="l00166"></a>00166         }
<a name="l00167"></a>00167         <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00168"></a>00168 }
<a name="l00169"></a>00169 
<a name="l00170"></a>00170 
</pre></div></div>
</div>
<hr class="footer"/><address class="footer"><small>Gegenereerd op Do 19 Mei 2011 10:15:28 voor Robocup-denayer-project-a door&nbsp;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.1 </small></address>
</body>
</html>
